﻿/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "decartwrist.h"
#include <math.h>
#include <stdlib.h>
#include <QDebug>

DecartWrist::DecartWrist(void)
{

}

DecartWrist::~DecartWrist(void)
{
}

int DecartWrist::initialForIk(double robotDH[6][4])
{

//初始化ＤＨ
    for(int i=0;i<6;i++)
    {
//        for(int j=0;j<4;j++)
//        {
//            m_DH[i][j]=robotDH[i][j];
//        }
        m_DH[i][0]=robotDH[i][3];
        m_DH[i][1]=robotDH[i][1];
        m_DH[i][2]=robotDH[i][0];
        m_DH[i][3]=robotDH[i][2];

    }


    //计算机器人结构参数
    eox=m_DH[0][2];
    eoy=m_DH[0][3];
    l1=m_DH[1][2];
    l2=sqrt(m_DH[2][2]*m_DH[2][2]+m_DH[3][3]*m_DH[3][3]);
    bJ3=acos(m_DH[2][2]/l2);


    //计算连杆转移矩阵
    calculateLinkMatrix(m_DH[3][0], m_DH[3][1], m_DH[3][2], m_DH[3][3],  A4);
    calculateLinkMatrix(m_DH[4][0], m_DH[4][1], m_DH[4][2], m_DH[4][3],  A5);
    calculateLinkMatrix(m_DH[5][0], m_DH[5][1], m_DH[5][2], m_DH[5][3],  A6);
  //  calculateLinkMatrix(m_DH[6][0], m_DH[6][1], m_DH[6][2], m_DH[6][3],  A7);
   // calculateLiMatrix(A7, A7LI);//转移矩阵的逆矩阵计算

    double MatrixC[4][4];
    multiplymatrix( A4, A5, MatrixC);
    multiplymatrix( MatrixC, A6, A456);

}



int DecartWrist::getIkSolutionFUT(double poseMatrix[4][4], int j4Turns, int j6Turns, bool isJ5Positive,
                                                                    double jointSolution[6])
{
    //计算1组J1J2J3
    if(1!=calculateJ1J2J3(poseMatrix,jointSolution))
    {
        return -1;
    }

    double jointOut1[3];
    double jointOut2[3];
    if(1!=calculateEulerJoint(poseMatrix,  jointOut1,jointOut2))
    {
        return -3;
    }

    //比较最优解。满足条件１）多圈问题。２）离初始点最近。
    double tmpBestAngle[3];
    if(1!=selectBestSolutionFUT(j4Turns, j6Turns, isJ5Positive, jointOut1,jointOut2, tmpBestAngle))
    {
        return -1;
    }
    jointSolution[3]=tmpBestAngle[0];
    jointSolution[4]=tmpBestAngle[1];
    jointSolution[5]=tmpBestAngle[2];


    return 1;
}

int DecartWrist::getIkSolutionFUT_5axisAc(double poseMatrix[4][4], int j4Turns, int j6Turns,
                                          bool isJ5Positive, double jointSolution[6])
{
    j6Turns=0;
    //计算1组J1J2J3
    if(1!=calculateJ1J2J3(poseMatrix,jointSolution))
    {
        return -1;
    }

    double jointOut1[3];
    double jointOut2[3];
    if(1!=calculateEulerJoint(poseMatrix,  jointOut1,jointOut2))
    {
        return -3;
    }

    //比较最优解。满足条件１）多圈问题。２）离初始点最近。
    double tmpBestAngle[3];
    if(1!=selectBestSolutionFUT(j4Turns, j6Turns, isJ5Positive, jointOut1,jointOut2, tmpBestAngle))
    {
        return -1;
    }
    jointSolution[3]=tmpBestAngle[0];
    jointSolution[4]=tmpBestAngle[1];
//    jointSolution[5]=tmpBestAngle[2];


    return 1;
}

int DecartWrist::getIkSolution(double poseMatrix[4][4], double beforeJoint[6], double jointSolution[6])//求位置逆解.
{
    //计算1组J1J2J3
    if(1!=calculateJ1J2J3(poseMatrix,jointSolution))
    {
        return -1;
    }

    double jointOut1[3];
    double jointOut2[3];
    if(1!=calculateEulerJoint(poseMatrix,  jointOut1,jointOut2))
    {
        return -3;
    }

    //比较最优解。满足条件１）多圈问题。２）离初始点最近。
    double tmpInitialAngle[3];
    tmpInitialAngle[0]=beforeJoint[3];
    tmpInitialAngle[1]=beforeJoint[4];
    tmpInitialAngle[2]=beforeJoint[5];
    double tmpBestAngle[3];
    if(1!=selectBestSolution(beforeJoint, jointOut1,jointOut2, tmpBestAngle))
    {
        return -1;
    }
    jointSolution[3]=tmpBestAngle[0];
    jointSolution[4]=tmpBestAngle[1];
    jointSolution[5]=tmpBestAngle[2];

    return 1;


}

int DecartWrist::getIkSolution_5axisAc(double poseMatrix[4][4], double beforeJoint[6], double jointSolution[6])
{
    //计算1组J1J2J3
    if(1!=calculateJ1J2J3(poseMatrix,jointSolution))
    {
        return -1;
    }

    double jointOut1[3];
    double jointOut2[3];
    if(1!=calculateEulerJoint(poseMatrix,  jointOut1,jointOut2))
    {
        return -3;
    }

    //比较最优解。满足条件１）多圈问题。２）离初始点最近。
    double tmpInitialAngle[3];
    tmpInitialAngle[0]=beforeJoint[3];
    tmpInitialAngle[1]=beforeJoint[4];
    tmpInitialAngle[2]=0;
    double tmpBestAngle[3];
    if(1!=selectBestSolution(beforeJoint, jointOut1,jointOut2, tmpBestAngle))
    {
        return -1;
    }
    jointSolution[3]=tmpBestAngle[0];
    jointSolution[4]=tmpBestAngle[1];
//    jointSolution[5]=tmpBestAngle[2];

    return 1;
}

void DecartWrist::multiplymatrix(double a[][4], double b[][4],double c[][4])//矩阵乘法函数
{
  c[0][0]=a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0]+a[0][3]*b[3][0];
  c[1][0]=a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0]+a[1][3]*b[3][0];
  c[2][0]=a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0]+a[2][3]*b[3][0];
  c[3][0]=a[3][0]*b[0][0]+a[3][1]*b[1][0]+a[3][2]*b[2][0]+a[3][3]*b[3][0];

  c[0][1]=a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1]+a[0][3]*b[3][1];
  c[1][1]=a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1]+a[1][3]*b[3][1];
  c[2][1]=a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1]+a[2][3]*b[3][1];
  c[3][1]=a[3][0]*b[0][1]+a[3][1]*b[1][1]+a[3][2]*b[2][1]+a[3][3]*b[3][1];

  c[0][2]=a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2]+a[0][3]*b[3][2];
  c[1][2]=a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2]+a[1][3]*b[3][2];
  c[2][2]=a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2]+a[2][3]*b[3][2];
  c[3][2]=a[3][0]*b[0][2]+a[3][1]*b[1][2]+a[3][2]*b[2][2]+a[3][3]*b[3][2];

  c[0][3]=a[0][0]*b[0][3]+a[0][1]*b[1][3]+a[0][2]*b[2][3]+a[0][3]*b[3][3];
  c[1][3]=a[1][0]*b[0][3]+a[1][1]*b[1][3]+a[1][2]*b[2][3]+a[1][3]*b[3][3];
  c[2][3]=a[2][0]*b[0][3]+a[2][1]*b[1][3]+a[2][2]*b[2][3]+a[2][3]*b[3][3];
  c[3][3]=a[3][0]*b[0][3]+a[3][1]*b[1][3]+a[3][2]*b[2][3]+a[3][3]*b[3][3];
}

void DecartWrist::calculateLinkMatrix(double oi, double afi, double ai, double di, double Ai[][4])//连杆转移矩阵计算
{

	Ai[0][0]=cos(oi);
	Ai[0][1]=-sin(oi)*cos(afi);
	Ai[0][2]=sin(oi)*sin(afi);
	Ai[0][3]=ai*cos(oi);

	Ai[1][0]=sin(oi);
	Ai[1][1]=cos(oi)*cos(afi);
	Ai[1][2]=-cos(oi)*sin(afi);
	Ai[1][3]=ai*sin(oi);

	Ai[2][0]=0;
	Ai[2][1]=sin(afi);
	Ai[2][2]=cos(afi);
	Ai[2][3]=di;

	Ai[3][0]=0;
	Ai[3][1]=0;
	Ai[3][2]=0;
	Ai[3][3]=1;

}

void DecartWrist::calculateLiMatrix(double a[][4], double b[][4])//转移矩阵的逆矩阵计算
{
	b[0][0]=a[0][0];
	b[0][1]=a[1][0];
	b[0][2]=a[2][0];
	b[0][3]=-(a[0][0]*a[0][3]+a[1][0]*a[1][3]+a[2][0]*a[2][3]);

	b[1][0]=a[0][1];
	b[1][1]=a[1][1];
	b[1][2]=a[2][1];
	b[1][3]=-(a[0][1]*a[0][3]+a[1][1]*a[1][3]+a[2][1]*a[2][3]);

	b[2][0]=a[0][2];
	b[2][1]=a[1][2];
	b[2][2]=a[2][2];
	b[2][3]=-(a[0][2]*a[0][3]+a[1][2]*a[1][3]+a[2][2]*a[2][3]);

	b[3][0]=0;
	b[3][1]=0;
	b[3][2]=0;
	b[3][3]=1;
	
}

int DecartWrist::calculateEulerJoint(double oritatationIn[][4], double jointOut1[], double jointOut2[])//用欧拉法,计算第i、i+1组解的J4J5J6
{
    double tmpJ2;
    tmpJ2=atan2(sqrt(oritatationIn[2][0]*oritatationIn[2][0]+oritatationIn[2][1]*oritatationIn[2][1]),oritatationIn[2][2]);//返回[-pi, pi]角度.

    //当J5=0时。实际应用可以取0.01°的精度。用0°时的2种解
    if(tmpJ2>-0.01*M_PI/180 && tmpJ2<0.01*M_PI/180 )
	{
        qDebug()<<"calculateEulerJoint ik solution failed: J5 singuraty1!"<<tmpJ2;
        return -3;
	}
	//当J5=180°舍弃。
    else if(  tmpJ2>M_PI-0.01*M_PI/180 || tmpJ2<-M_PI+0.01*M_PI/180 )
	{
        qDebug()<<"calculateEulerJoint solution failed: J5 singuraty2!"<<tmpJ2;
        return -4;
	}
	else//当J5不等于0或180°时
	{
        //计算第1组解
        jointOut1[0]=atan2(oritatationIn[1][2]/sin(tmpJ2),oritatationIn[0][2]/sin(tmpJ2));//返回[-pi, pi]角度.
        jointOut1[1]=tmpJ2;
        jointOut1[2]=atan2(oritatationIn[2][1]/sin(tmpJ2),-oritatationIn[2][0]/sin(tmpJ2));//返回[-pi, pi]角度.

        //计算第2组解
        jointOut2[0]=jointOut1[0]+M_PI;
        jointOut2[1]=-jointOut1[1];
        jointOut2[2]=jointOut1[2]+M_PI;

	}
	
    return 1;

}


void DecartWrist::calculatePosition(double DH[][4], double J[][6], int i, double P[][4])//位置姿态运动学正解
{

	//连杆转移矩阵
    double A1[4][4],A2[4][4],A3[4][4],A4[4][4],A5[4][4],A6[4][4],c[4][4],d[4][4];

	calculateLinkMatrix(J[i][0], DH[0][1], DH[0][2], DH[0][3],  A1);
	calculateLinkMatrix(J[i][1], DH[1][1], DH[1][2], DH[1][3],  A2);
	calculateLinkMatrix(J[i][2], DH[2][1], DH[2][2], DH[2][3],  A3);
	calculateLinkMatrix(J[i][3], DH[3][1], DH[3][2], DH[3][3],  A4);
	calculateLinkMatrix(J[i][4], DH[4][1], DH[4][2], DH[4][3],  A5);
	calculateLinkMatrix(J[i][5], DH[5][1], DH[5][2], DH[5][3],  A6);
    //calculateLinkMatrix(m_DH[6][0], m_DH[6][1], m_DH[6][2], m_DH[6][3],  A7);

	multiplymatrix( A1, A2, c);
	multiplymatrix( c, A3, d);
	multiplymatrix( d, A4, c);
	multiplymatrix( c, A5, d);
	multiplymatrix( d, A6, c);
	//multiplymatrix( d, A6, P);
    //multiplymatrix( c, A7, P);

}

int DecartWrist::calculatePosition(double jointValue[6], double poseMatrix[][4])
{
		//连杆转移矩阵
    double A1[4][4],A2[4][4],A3[4][4],A4[4][4],A5[4][4],A6[4][4],c[4][4],d[4][4];

    calculateLinkMatrix(jointValue[0], m_DH[0][1], m_DH[0][2], m_DH[0][3],  A1);
    jointValue[1] += M_PI_2;
    calculateLinkMatrix(jointValue[1], m_DH[1][1], m_DH[1][2], m_DH[1][3],  A2);
    calculateLinkMatrix(jointValue[2], m_DH[2][1], m_DH[2][2], m_DH[2][3],  A3);
    calculateLinkMatrix(jointValue[3], m_DH[3][1], m_DH[3][2], m_DH[3][3],  A4);
    calculateLinkMatrix(jointValue[4], m_DH[4][1], m_DH[4][2], m_DH[4][3],  A5);
    calculateLinkMatrix(jointValue[5], m_DH[5][1], m_DH[5][2], m_DH[5][3],  A6);

	multiplymatrix( A1, A2, c);
	multiplymatrix( c, A3, d);
	multiplymatrix( d, A4, c);
	multiplymatrix( c, A5, d);
    multiplymatrix( d, A6, poseMatrix);

    return 1;
}

int DecartWrist::getBestPositionFromAngle(double idealAngle, double initialPosition, double &bestPositionOut)
{
    int tmpKey= getBestPositionFromAngle( idealAngle/M_PI*180.0, initialPosition/M_PI*180.0, bestPositionOut);
    bestPositionOut=bestPositionOut/180.0*M_PI;
    return tmpKey;
}

int DecartWrist::getBestPositionFromAngle_Rad(double idealAngle, double initialPosition, double &bestPositionOut)
{
    int currentTurns;
    if(0<initialPosition)
    {
        currentTurns=(initialPosition+180) /(360);
    }
    else
    {
        currentTurns=(initialPosition-180) /(360);
    }

    double angle1,angle2,angle3;
    angle1=currentTurns*360+idealAngle;
    angle2=angle1+360;
    angle3=angle1-360;
    double offset1,offset2,offset3;
    double minNum;
    int minIndex;
    offset1=fabs(angle1-initialPosition);
    offset2=fabs(angle2-initialPosition);
    offset3=fabs(angle3-initialPosition);
    if(offset1<=offset2)
    {
        minNum=offset1;
        minIndex=1;
    }
    else
    {
        minNum=offset2;
        minIndex=2;
    }

    if(offset3<=minNum)
    {
        minNum=offset3;
        minIndex=3;
    }
    switch(minIndex)
    {
    case 1:
    {
        bestPositionOut=angle1;
        break;

    }
    case 2:
    {
        bestPositionOut=angle2;
        break;

    }
    default:
    {
        bestPositionOut=angle3;
    }
    }


    return 1;
}


int DecartWrist::calculateJ1(void)//计算8组J1
{

    //判断是否有顶置奇异点
    if( (m_T60[1][3]<0.02 && m_T60[1][3]>-0.02)
            && (m_T60[0][3]<0.02 && m_T60[0][3]>-0.02))
    {
        //qDebug()<<"six robot ik solution failed: top singuraty!";
        return -1;
    }
	m_J[0][0]=atan2(m_T60[1][3],m_T60[0][3]);//nearJ1  返回[-pi, pi]角度.
	m_J[1][0]=m_J[0][0];//nearJ1
	m_J[2][0]=m_J[0][0];//nearJ1
	m_J[3][0]=m_J[0][0];//nearJ1

	m_J[4][0]=m_J[0][0]+M_PI;//farJ1
	m_J[5][0]=m_J[4][0];//farJ1
	m_J[6][0]=m_J[4][0];//farJ1
	m_J[7][0]=m_J[4][0];//farJ1

    return 1;


}

int DecartWrist::calculateJ1J2J3(double poseMatrix[4][4],double jointSolution[])
{
    jointSolution[0]=poseMatrix[0][3];
    jointSolution[1]=poseMatrix[1][3];
    jointSolution[2]=poseMatrix[2][3];
    return 1;
}

int DecartWrist::calculateJ2J3(void)//计算8组J2J3
{
	//当J1为近位置时*******
    double ox=sqrt(m_T60[1][3]*m_T60[1][3]+m_T60[0][3]*m_T60[0][3]);
    double oy=m_T60[2][3];
    double x1=ox-eox;
    double y1=oy-eoy;
    double b=atan2(y1,x1);//弦角度 返回[-pi, pi]角度.
    double angleRange=(x1*x1+y1*y1+l1*l1-l2*l2)/(2*l1*sqrt(x1*x1+y1*y1));
    double a;

    if(angleRange>=-0.999 && angleRange<=0.999)//检验目标点是否超出活动范围,离手臂成直线还差?度
	{
		a=acos(angleRange);//增减角，返回[0,180]范围内的角度
		m_J[0][1]=b-a;      //小J2，对应正的J3角度
		m_J[1][1]=m_J[0][1];//小J2，对应正的J3角度

		m_J[2][1]=b+a;      //大J2,对应负的J3角度
		m_J[3][1]=m_J[2][1];//大J2,对应负的J3角度

		m_J[0][2]=acos((x1*x1+y1*y1-l1*l1-l2*l2)/(2*l1*l2));//正J3
		m_J[1][2]=m_J[0][2];                                //正J3

		m_J[2][2]=-m_J[1][2];//负J3
		m_J[3][2]=m_J[2][2]; //负J3
	}
	else
	{
		m_JIsValidate[0]=false;
		m_JIsValidate[1]=false;
		m_JIsValidate[2]=false;
		m_JIsValidate[3]=false;

	}


    //当J1为远位置时********
	x1=ox+eox;
	b=atan2(y1,x1);
	angleRange=(x1*x1+y1*y1+l1*l1-l2*l2)/(2*l1*sqrt(x1*x1+y1*y1));

    if(angleRange>=-0.999 && angleRange<=0.999)//检验目标点是否超出活动范围
	{
		a=acos(angleRange);//增减角
		m_J[4][1]=M_PI-(b+a);//小J2，对应正的J3角度
		m_J[5][1]=m_J[4][1];//小J2，对应正的J3角度

		m_J[6][1]=M_PI-(b-a);//大J2,对应负的J3角度
		m_J[7][1]=m_J[6][1];//大J2,对应负的J3角度

		m_J[4][2]=acos((x1*x1+y1*y1-l1*l1-l2*l2)/(2*l1*l2));//正J3
		m_J[5][2]=m_J[4][2];                                //正J3

		m_J[6][2]=-m_J[5][2];//负J3
		m_J[7][2]=m_J[6][2]; //负J3

	}
	else
	{
		m_JIsValidate[4]=false;
		m_JIsValidate[5]=false;
		m_JIsValidate[6]=false;
		m_JIsValidate[7]=false;

	}

    // J3 singuraty!
    if(m_JIsValidate[0]==false && m_JIsValidate[1]==false && m_JIsValidate[2]==false &&m_JIsValidate[3]==false &&
            m_JIsValidate[4]==false && m_JIsValidate[5]==false && m_JIsValidate[6]==false &&m_JIsValidate[7]==false )
    {
        //qDebug()<<"six robot ik solution failed: J3 singuraty!";
        return -2;
    }


	m_J[0][2]=m_J[0][2]+bJ3;//更新J3为标准DH角度
	m_J[1][2]=m_J[1][2]+bJ3;//更新J3为标准DH角度
	m_J[2][2]=m_J[2][2]+bJ3;//更新J3为标准DH角度
	m_J[3][2]=m_J[3][2]+bJ3;//更新J3为标准DH角度
	m_J[4][2]=m_J[4][2]+bJ3;//更新J3为标准DH角度
	m_J[5][2]=m_J[5][2]+bJ3;//更新J3为标准DH角度
	m_J[6][2]=m_J[6][2]+bJ3;//更新J3为标准DH角度
	m_J[7][2]=m_J[7][2]+bJ3;//更新J3为标准DH角度

    return 1;

}

void DecartWrist::calculateEulerTransferMatrix(int i)//第i组解的连杆转移矩阵
{
		
	calculateLinkMatrix(m_J[i][0], m_DH[0][1], m_DH[0][2], m_DH[0][3],  A1);
	calculateLinkMatrix(m_J[i][1], m_DH[1][1], m_DH[1][2], m_DH[1][3],  A2);
	calculateLinkMatrix(m_J[i][2], m_DH[2][1], m_DH[2][2], m_DH[2][3],  A3);

    double MatrixC[4][4];
	multiplymatrix( A1, A2, MatrixC);
	multiplymatrix( MatrixC, A3, A123);
	multiplymatrix( A123, A456, A1_6);//求得A1*A2*A3*A4*A5*A6

	calculateLiMatrix(A1_6, A1_6LI);//求A1_6的逆矩阵

    multiplymatrix( A1_6LI, m_T60, T456);//求得第2组解的连杆456的目标转移矩阵T456，位置元素误差百分比大

}

void DecartWrist::MatrixRotationXself(double Matrixa[][4], double angle)
{
    double a[4][4]={{1,0,0,0}, {0,cos(angle),-sin(angle),0}, { 0,sin(angle),cos(angle),0}, { 0,0,0,1} };
    double b[4][4];
	multiplymatrix(Matrixa,a,b);

	for(int i=0;i<4;i++)
   {
	   	for(int j=0;j<4;j++)
		{
			Matrixa[i][j]=b[i][j];
		  
		}
	  
	}

}
void DecartWrist::MatrixRotationYself(double Matrixa[][4], double angle)
{
    double a[4][4]={{cos(angle),0,sin(angle),0}, {0,1,0,0}, { -sin(angle),0,cos(angle),0}, { 0,0,0,1} };
    double b[4][4];
	multiplymatrix(Matrixa,a,b);

	for(int i=0;i<4;i++)
   {
	   	for(int j=0;j<4;j++)
		{
			Matrixa[i][j]=b[i][j];
		  
		}
	  
	}

}

void DecartWrist::MatrixRotationZself(double Matrixa[][4], double angle)
{
    double a[4][4]={{cos(angle),-sin(angle),0,0}, {sin(angle),cos(angle),0,0}, { 0,0,1,0}, { 0,0,0,1} };
    double b[4][4];
	multiplymatrix(Matrixa,a,b);

	for(int i=0;i<4;i++)
   {
	   	for(int j=0;j<4;j++)
		{
			Matrixa[i][j]=b[i][j];
		  
		}
	  
	}

}
void DecartWrist::MatrixTranslateSelfcoordinate(double Matrixa[][4],double x,double y,double z)//参考TCP坐标平移
{
    double a[4][4]={{1,0,0,x}, {0,1,0,y}, { 0,0,1,z}, { 0,0,0,1} };
    double b[4][4];
	multiplymatrix(Matrixa,a,b);

	for(int i=0;i<4;i++)
   {
	   	for(int j=0;j<4;j++)
		{
			Matrixa[i][j]=b[i][j];
		  
		}
	  
	}

}
void DecartWrist::MatrixTranslateWorldcoordinate(double Matrixa[][4],double x,double y,double z)//参考世界坐标平移
{
	Matrixa[0][3]+=x;
	Matrixa[1][3]+=y;
	Matrixa[2][3]+=z;
}


void DecartWrist::MatrixEqual(double a[][4], double b[][4])//b<=a
{
for(int i=0;i<4;i++)
   {
	   	for(int j=0;j<4;j++)
		{
			b[i][j]=a[i][j];
		  
		}
	  
	}
}

void DecartWrist::MatrixEqual(double a[][4], double b[][4],int n)
{
for(int i=0;i<n;i++)
   {
	   	for(int j=0;j<4;j++)
		{
			b[i][j]=a[i][j];
		  
		}
	  
	}
}


void DecartWrist::TransferDirectJToDHJ(double DJ[6], double DHJ[6],bool IsDirectAngle2DHJAngle)
{
	  if(IsDirectAngle2DHJAngle)
	  {
	  DHJ[0]=-DJ[0];
	  DHJ[1]=DJ[1]+M_PI_2;
	  DHJ[2]=-DJ[2];
	  DHJ[3]=DJ[3];
	  DHJ[4]=-DJ[4];
	  DHJ[5]=-DJ[5];
	  }
	  else
	  {
	  DJ[0]=-DHJ[0];
	  DJ[1]=DHJ[1]-M_PI_2;
	  DJ[2]=-DHJ[2];
	  DJ[3]=DHJ[3];
	  DJ[4]=-DHJ[4];
	  DJ[5]=-DHJ[5];

	  }


}
void DecartWrist::TransferDirectJToDHJ(double DJ[6], double DHJ1,double DHJ2,double DHJ3,double DHJ4,double DHJ5,double DHJ6,bool IsDirectAngle2DHJAngle)
{
	  if(IsDirectAngle2DHJAngle)
	  {
	  DHJ1=-DJ[0];
	  DHJ2=DJ[1]+M_PI_2;
	  DHJ3=-DJ[2];
	  DHJ4=DJ[3];
	  DHJ5=-DJ[4];
	  DHJ6=-DJ[5];
	  }
	  else
	  {
	  DJ[0]=-DHJ1;
	  DJ[1]=DHJ2-M_PI_2;
	  DJ[2]=-DHJ3;
	  DJ[3]=DHJ4;
	  DJ[4]=-DHJ5;
	  DJ[5]=-DHJ6;

	  }


}

int DecartWrist::selectBestSolutionFUT(int j4Turns,int j6Turns,bool isJ5Positive,
                                       double jointIn1[3],double jointIn2[3],double jointOut[3])
{

    if(isJ5Positive)
    {
        if(jointIn1[2]>0)
        {
            for(int n=0;n<3;n++)
            {
                jointOut[n]=jointIn1[n];
            }
        }
        else
        {
            for(int n=0;n<3;n++)
            {
                jointOut[n]=jointIn2[n];
            }
        }
    }
    else
    {
        if(jointIn1[2]>0)
        {
            for(int n=0;n<3;n++)
            {
                jointOut[n]=jointIn2[n];
            }
        }
        else
        {
            for(int n=0;n<3;n++)
            {
                jointOut[n]=jointIn1[n];
            }
        }
    }
    jointOut[0]+=j4Turns*M_PI*2;
    jointOut[2]+=j6Turns*M_PI*2;



    return 1;
}

double DecartWrist::transformToPi(double angleIn)
{
    //    int turns=angleIn/M_PI/2;//可能bug 参考getTurnsFromRad
        int turns;

        if(0<angleIn)
        {
            turns=(angleIn+M_PI) /(2*M_PI);
        }
        else
        {
            turns=(angleIn-M_PI) /(2*M_PI);
        }

        //    angleIn=angleIn-turns*M_PI;//错误
            angleIn=angleIn-turns*M_PI*2;

        if(angleIn>=-M_PI && angleIn<=M_PI)
        {
            return angleIn;
        }
        else if(angleIn<-M_PI)
        {
            return angleIn+2*M_PI;
        }
        else if(angleIn>M_PI)
        {
            return angleIn-2*M_PI;
        }

        return angleIn;
}


int DecartWrist::getTurnsFromDegree(double degreeIn)
{
    //    int turns;
    //    degreeIn = (degreeIn+180.0) /360.0;
    //    if(degreeIn>0.0){
    //        turns = floor(degreeIn);//往下取整
    //    }
    //    else{
    //        turns = ceil(degreeIn);//往上取整
    //    }

        return getTurnsFromRad(degreeIn/180.0*M_PI);
}

int DecartWrist::getTurnsFromRad(double degreeIn)
{
    int turns;
//    degreeIn = (degreeIn+M_PI) /(2*M_PI);
//    if(degreeIn>0.0){
//        turns = floor(degreeIn);//往下取整
//    }
//    else{
//        turns = ceil(degreeIn);//往上取整
//    }
    if(0<degreeIn)
    {
        turns=(degreeIn+M_PI) /(2*M_PI);
    }
    else
    {
        turns=(degreeIn-M_PI) /(2*M_PI);
    }

    return turns;
}

double DecartWrist::getTrimDegreeFromDegree(double degreeIn)
{
    int turns=getTurnsFromDegree(degreeIn);
    double trimDegree=degreeIn-360.0*turns;
    return trimDegree;

}

double DecartWrist::getTrimRadFromRad(double degreeIn)
{
    int turns=getTurnsFromRad(degreeIn);
    double trimDegree=degreeIn-2*M_PI*turns;
    return trimDegree;

}

int DecartWrist::selectBestSolution(double beforeJoint[3], double jointIn1[3], double jointIn2[3], double jointOut[3])
{

    //先除去多圈，使在[-pi,pi]区间
    for(int n=0;n<3;n++)
	{
        beforeJoint[n]=transformToPi(beforeJoint[n]);
	}


    //判断角度变换，来决定选择解。
    double tmpSum1=0;
    double tmpSum2=0;
    for(int n=0;n<3;n++)
    {
        tmpSum1+=fabs(angleRotationJudgement(beforeJoint[n],jointIn1[n]));
    }
    for(int n=0;n<3;n++)
    {
        tmpSum2+=fabs(angleRotationJudgement(beforeJoint[n],jointIn2[n]));
    }

    if(tmpSum1<tmpSum2)
    {
        for(int n=0;n<3;n++)
        {
            jointOut[n]=jointIn1[n];
        }
    }
    else
    {
        for(int n=0;n<3;n++)
        {
            jointOut[n]=jointIn2[n];
        }
    }

    //多圈
    for(int n=0;n<3;n++)
    {
         getBestPositionFromAngle_Rad(jointOut[n],beforeJoint[n],jointOut[n]);
    }

    return 1;

}

double DecartWrist::angleRotationJudgement(double targetAngle, double initialAngle)
{
    double angle1,angle2,angle3;
    angle1=targetAngle-initialAngle;
    angle2=angle1+2*M_PI;
    angle3=angle1-2*M_PI;
    double minNum;
    if(fabs(angle1)<=fabs(angle2))
    {
        minNum=angle1;
    }
    else
    {
        minNum=angle2;
    }

    if(fabs(angle3)<=fabs(minNum))
    {
        minNum=angle3;
    }

    return minNum;
}

double DecartWrist::MaxCompare(double x, double y, double z)
{
         double p,q;
		 
		 if(x<y) p=y;
		 else p=x;
		 
		 if(z<p) q = p;
		 else q = z;
		 return q;
}









